#include "Config.h"

void Init_Enc_Arm_Sensor_src(void)
{
	if(FIELD==Blue_Field)
	{
		src_deg_Blue= nMotorEncoder[Mtr_Arm];

		dst_deg_Blue=src_deg_Blue+delta_src_dst;

		src_deg=src_deg_Blue;
		dst_deg=dst_deg_Blue;
	}
	else
	{
		src_deg_Red= nMotorEncoder[Mtr_Arm];
		dst_deg_Red=src_deg_Red-delta_src_dst;

		src_deg=src_deg_Red;
		dst_deg=dst_deg_Red;
	}
}
/////////////////
void Init_Enc_Arm_Sensor_dst(void)
{
	if(FIELD==Blue_Field)
	{
		dst_deg_Blue= nMotorEncoder[Mtr_Arm];
		src_deg_Blue=dst_deg_Blue-delta_src_dst;

		src_deg=src_deg_Blue;
		dst_deg=dst_deg_Blue;
	}
	else
	{
		dst_deg_Red= nMotorEncoder[Mtr_Arm];
		src_deg_Red=dst_deg_Red+delta_src_dst;

		src_deg=src_deg_Red;
		dst_deg=dst_deg_Red;
	}
}
////////////////
void Init_Position()
{
	Claw_Statue_Func(Claw_Open);
	Rise_UpDown_Func(-35);
	wait1Msec(1000);
}
